{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import yaml\n",
    "f = open('inspire_hand/config.yaml')\n",
    "try:\n",
    "    config = yaml.load(f, Loader=yaml.FullLoader)\n",
    "finally:\n",
    "    f.close()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[INFO] Remove link left_rubber_hand\n",
      "[INFO] Remove link right_rubber_hand\n",
      "[INFO] Remove joint left_hand_palm_joint\n",
      "[INFO] Remove joint right_hand_palm_joint\n",
      "[INFO] Generate merged URDF file: g1_29dof_rev_1_0_with_inspire_hand_FTP.urdf Done.\n",
      "[INFO] Remove link left_rubber_hand\n",
      "[INFO] Remove link right_rubber_hand\n",
      "[INFO] Remove joint left_hand_palm_joint\n",
      "[INFO] Remove joint right_hand_palm_joint\n",
      "[INFO] Generate merged URDF file: g1_29dof_rev_1_0_with_inspire_hand_DFQ.urdf Done.\n"
     ]
    }
   ],
   "source": [
    "import xml.etree.ElementTree as ET\n",
    "\n",
    "\n",
    "def merge_urdfs(g1_urdf, hand_type):\n",
    "    lhand_urdf = f\"inspire_hand/{hand_type}_left_hand.urdf\"\n",
    "    rhand_urdf = f\"inspire_hand/{hand_type}_right_hand.urdf\"\n",
    "    G1 = ET.parse(g1_urdf)\n",
    "    g1 = G1.getroot()\n",
    "    lhand = ET.parse(lhand_urdf).getroot()\n",
    "    rhand = ET.parse(rhand_urdf).getroot()\n",
    "\n",
    "    # clean\n",
    "    for element in g1:\n",
    "        name = element.attrib.get('name', None)\n",
    "        if element.tag == \"link\" and name in config['G1_remove_links']:\n",
    "            print('[INFO] Remove link', name)\n",
    "            g1.remove(element)\n",
    "\n",
    "    for element in g1:\n",
    "        name = element.attrib.get('name', None)\n",
    "        if element.tag == \"joint\" and name in config['G1_remove_joints']:\n",
    "            print('[INFO] Remove joint', name)\n",
    "            g1.remove(element)\n",
    "\n",
    "    for element in lhand:\n",
    "        name = element.attrib.get('name', None)\n",
    "        if element.tag == \"link\" and name in config['L_hand_remove_links']:\n",
    "            lhand.remove(element)\n",
    "    for element in rhand:\n",
    "        name = element.attrib.get('name', None)\n",
    "        if element.tag == \"link\" and name in config['R_hand_remove_links']:\n",
    "            rhand.remove(element)\n",
    "\n",
    "    # merge\n",
    "    for element in lhand:\n",
    "        if element.tag in [\"link\", \"joint\"]:\n",
    "            g1.append(element)\n",
    "    for element in rhand:\n",
    "        if element.tag in [\"link\", \"joint\"]:\n",
    "            g1.append(element)\n",
    "\n",
    "    output = g1_urdf[:-5] + f\"_with_inspire_hand_{hand_type}.urdf\"\n",
    "    G1.write(output)\n",
    "    print(f\"[INFO] Generate merged URDF file: {output} Done.\")\n",
    "\n",
    "\n",
    "merge_urdfs(g1_urdf=\"g1_29dof_rev_1_0.urdf\", hand_type=\"FTP\")\n",
    "merge_urdfs(g1_urdf=\"g1_29dof_rev_1_0.urdf\", hand_type=\"DFQ\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "rerun3x",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
